#ifndef VECTOR3_HPP_
#define VECTOR3_HPP_

#include <cmath>
#include <iomanip>
#include <iostream>
#include <sstream>
#include <string>

#include "Vector2.hpp"

class Rotation;

class Vector3 {

public:
  Vector3(double x = 0.0, double y = 0.0, double z = 0.0);
  // The constructor.
  
  Vector3(const double *); //swig ignore
  Vector3(const float *);  //swig ignore
   // Constructors from an array

   Vector3(const Vector3 &);
   // The copy constructor.

   virtual ~Vector3();
   // Destructor

   double operator () (int) const;
   inline double operator [] (int) const;
   // Get components by index (Geant4).

   double & operator () (int);
   inline double & operator [] (int);
   // Set components by index.

   inline double x()  const;
   inline double y()  const;
   inline double z()  const;
   inline double X()  const;
   inline double Y()  const;
   inline double Z()  const;
   inline double Px() const;
   inline double Py() const;
   inline double Pz() const;
   // The components in cartesian coordinate system.

   inline void SetX(double);
   inline void SetY(double);
   inline void SetZ(double);
   inline void SetXYZ(double x, double y, double z);
   void        SetPtEtaPhi(double pt, double eta, double phi);
   void        SetPtThetaPhi(double pt, double theta, double phi);
  
   inline void GetXYZ(double *carray) const;
   inline void GetXYZ(float *carray) const;
   // Get the components into an array
   // not checked!

   double Phi() const;
   // The azimuth angle. returns phi from -pi to pi 

   double Theta() const;
   // The polar angle.

   inline double CosTheta() const;
   // Cosine of the polar angle.

   inline double Mag2() const;
   // The magnitude squared (rho^2 in spherical coordinate system).

   double Mag() const;
   // The magnitude (rho in spherical coordinate system).

   void SetPhi(double);
   // Set phi keeping mag and theta constant (BaBar).

   void SetTheta(double);
   // Set theta keeping mag and phi constant (BaBar).

   inline void SetMag(double);
   // Set magnitude keeping theta and phi constant (BaBar).

   inline double Perp2() const;
   // The transverse component squared (R^2 in cylindrical coordinate system).

   inline double Pt() const;
   double Perp() const;
   // The transverse component (R in cylindrical coordinate system).

   inline void SetPerp(double);
   // Set the transverse component keeping phi and z constant.

   inline double Perp2(const Vector3 &) const;
   // The transverse component w.r.t. given axis squared.

   inline double Pt(const Vector3 &) const;
   double Perp(const Vector3 &) const;
   // The transverse component w.r.t. given axis.

   inline double DeltaPhi(const Vector3 &) const;
   double DeltaR(const Vector3 &) const;
   inline double DrEtaPhi(const Vector3 &) const;
   inline Vector2 EtaPhiVector() const;
   void SetMagThetaPhi(double mag, double theta, double phi);

   inline Vector3 & operator = (const Vector3 &);
   // Assignment.

   inline bool operator == (const Vector3 &) const;
   inline bool operator != (const Vector3 &) const;
   // Comparisons (Geant4).

   inline Vector3 & operator += (const Vector3 &);
   // Addition.

   inline Vector3 & operator -= (const Vector3 &);
   // Subtraction.

   inline Vector3 operator - () const;
   // Unary minus.

   inline Vector3 & operator *= (double);
   // Scaling with real numbers.

  Vector3 operator+(const Vector3& rhs) {
    return Vector3(fX + rhs.fX, fY + rhs.fY, fZ + rhs.fZ);
  }

  Vector3 operator-(const Vector3& rhs) {
    return Vector3(fX - rhs.fX, fY - rhs.fY, fZ - rhs.fZ);
  }
  
  double operator*(const Vector3& rhs) {
    return fX*rhs.fX + fY*rhs.fY + fZ*rhs.fZ;
  }

  Vector3 operator*(double s) {
    return Vector3(fX*s, fY*s, fZ*s);
  }

  Vector3 operator/(double s) {
    return Vector3(fX/s, fY/s, fZ/s);
  }

   Vector3 Unit() const;
   // Unit vector parallel to this.

   inline Vector3 Orthogonal() const;
   // Vector orthogonal to this (Geant4).

   inline double Dot(const Vector3 &) const;
   // Scalar product.

   inline Vector3 Cross(const Vector3 &) const;
   // Cross product.

   double Angle(const Vector3 &) const;
   // The angle w.r.t. another 3-vector.

   double PseudoRapidity() const;
   // Returns the pseudo-rapidity, i.e. -ln(tan(theta/2))

   inline double Eta() const;

   void RotateX(double);
   // Rotates the Hep3Vector around the x-axis.

   void RotateY(double);
   // Rotates the Hep3Vector around the y-axis.

   void RotateZ(double);
   // Rotates the Hep3Vector around the z-axis.

   void RotateUz(const Vector3&);
   // Rotates reference frame from Uz to newUz (unit vector) (Geant4).

   void Rotate(double, const Vector3 &);
   // Rotates around the axis specified by another Hep3Vector.

  Vector3 & operator *= (const Rotation &);
  Vector3 & Transform(const Rotation &);
  // Transformation with a Rotation matrix.
  
  inline Vector2 XYvector() const;
  
  std::string ToString();

private:

   double fX, fY, fZ;
   // The components.

};

Vector3 operator + (const Vector3 &, const Vector3 &);
// Addition of 3-vectors.

Vector3 operator - (const Vector3 &, const Vector3 &);
// Subtraction of 3-vectors.

double operator * (const Vector3 &, const Vector3 &);
// Scalar product of 3-vectors.

Vector3 operator * (const Vector3 &, double a);
Vector3 operator * (double a, const Vector3 &);
// Scaling of 3-vectors with a real number

class Rotation {
    
public:

#ifndef SWIG
class RotationRow {
public:
   inline RotationRow(const Rotation &, int);
   inline RotationRow(const RotationRow &);
   inline RotationRow & operator=(const RotationRow &);
   inline double operator [] (int) const;
private:
   const Rotation * fRR;
   //    const Rotation & fRR;
   int fII;
};
#endif
   // Helper class for implemention of C-style subscripting r[i][j]

   Rotation();
   // Default constructor. Gives a unit matrix.

   Rotation(const Rotation &);

   // Copy constructor.

   virtual ~Rotation() {}

   inline double XX() const;
   inline double XY() const;
   inline double XZ() const;
   inline double YX() const;
   inline double YY() const;
   inline double YZ() const;
   inline double ZX() const;
   inline double ZY() const;
   inline double ZZ() const;
   // Elements of the rotation matrix (Geant4).

   inline RotationRow operator [] (int) const;
   // Returns object of the helper class for C-style subscripting r[i][j]

   double operator () (int, int) const;
   // Fortran-style subscripting: returns (i,j) element of the rotation matrix.

   inline Rotation & operator = (const Rotation &);
   // Assignment.

   inline bool operator == (const Rotation &) const;
   inline bool operator != (const Rotation &) const;
   // Comparisons (Geant4).

   inline bool IsIdentity() const;
   // Returns true if the identity matrix (Geant4).

   inline Vector3 operator * (const Vector3 &) const;  // Vector3
   // Multiplication with a Vector3.

   Rotation operator * (const Rotation &) const;
   inline Rotation & operator *= (const Rotation &);
   inline Rotation & Transform(const Rotation &);
   // Matrix multiplication.
   // Note a *= b; <=> a = a * b; while a.transform(b); <=> a = b * a;

   inline Rotation Inverse() const;
   // Returns the inverse.

   inline Rotation & Invert();
   // Inverts the Rotation matrix.

   Rotation & RotateX(double);
   // Rotation around the x-axis.

   Rotation & RotateY(double);
   // Rotation around the y-axis.

   Rotation & RotateZ(double);
   // Rotation around the z-axis.
 
   Rotation & Rotate(double, const Vector3 &);
   inline Rotation & Rotate(double, const Vector3 *);
   // Rotation around a specified vector.

   Rotation & RotateAxes(const Vector3 & newX,
                          const Vector3 & newY,
                          const Vector3 & newZ);
   // Rotation of local axes (Geant4).

   double PhiX() const;
   double PhiY() const;
   double PhiZ() const;
   double ThetaX() const;
   double ThetaY() const;
   double ThetaZ() const;
   // Return angles (RADS) made by rotated axes against original axes (Geant4).

   void AngleAxis(double &, Vector3 &) const;
   // Returns the rotation angle and rotation axis (Geant4).

   inline Rotation & SetToIdentity();
   // Set equal to the identity rotation.
 
   Rotation & SetXEulerAngles(double phi, double theta, double psi);
   void SetXPhi(double);
   void SetXTheta(double);
   void SetXPsi(double);
   // Set the euler angles of the rotation.  The angles are defined using the
   // y-convention which rotates around the Z axis, around the new X axis, and
   // then around the new Z axis.  The x-convention is used Goldstein, Landau
   // and Lifshitz, and other common physics texts.  Contrast this with
   // SetYEulerAngles.

   Rotation & RotateXEulerAngles(double phi, double theta, double psi);
   // Adds a rotation of the local axes defined by the Euler angle to the
   // current rotation.  See SetXEulerAngles for a note about conventions.

   double GetXPhi(void) const;
   double GetXTheta(void) const;
   double GetXPsi(void) const;
   // Return the euler angles of the rotation.  See SetYEulerAngles for a
   // note about conventions.

   Rotation & SetYEulerAngles(double phi, double theta, double psi);
   void SetYPhi(double);
   void SetYTheta(double);
   void SetYPsi(double);
   // Set the euler angles of the rotation.  The angles are defined using the
   // y-convention which rotates around the Z axis, around the new Y axis, and
   // then around the new Z axis.  The x-convention is used Goldstein, Landau
   // and Lifshitz, and other common physics texts and is a rotation around the
   // Z axis, around the new X axis, and then around the new Z axis.

   Rotation & RotateYEulerAngles(double phi, double theta, double psi);
   // Adds a rotation of the local axes defined by the Euler angle to the
   // current rotation.  See SetYEulerAngles for a note about conventions.

   double GetYPhi(void) const;
   double GetYTheta(void) const;
   double GetYPsi(void) const;
   // Return the euler angles of the rotation.  See SetYEulerAngles for a
   // note about conventions.

   Rotation & SetXAxis(const Vector3& axis);
   Rotation & SetXAxis(const Vector3& axis, const Vector3& xyPlane);
   Rotation & SetYAxis(const Vector3& axis);
   Rotation & SetYAxis(const Vector3& axis, const Vector3& yzPlane);
   Rotation & SetZAxis(const Vector3& axis);
   Rotation & SetZAxis(const Vector3& axis, const Vector3& zxPlane);
   // Create a rotation with the axis vector parallel to the rotated coordinate
   // system.  If a second vector is provided it defines a plane passing
   // through the axis.

   void MakeBasis(Vector3& xAxis, Vector3& yAxis, Vector3& zAxis) const;
   // Take two input vectors (in xAxis, and zAxis) and turn them into an
   // orthogonal basis.  This is an internal helper function used to implement
   // the Set?Axis functions, but is exposed because the functionality is 
   // often useful.

  std::string ToString();

protected:

   Rotation(double, double, double, double, double,
             double, double, double, double);
   // Protected constructor.

   double fxx, fxy, fxz, fyx, fyy, fyz, fzx, fzy, fzz;
   // The matrix elements.


};


double & Vector3::operator[] (int i)       { return operator()(i); }
double   Vector3::operator[] (int i) const { return operator()(i); }

inline double Vector3::x()  const { return fX; }
inline double Vector3::y()  const { return fY; }
inline double Vector3::z()  const { return fZ; }
inline double Vector3::X()  const { return fX; }
inline double Vector3::Y()  const { return fY; }
inline double Vector3::Z()  const { return fZ; }
inline double Vector3::Px() const { return fX; }
inline double Vector3::Py() const { return fY; }
inline double Vector3::Pz() const { return fZ; }

inline void Vector3::SetX(double xx) { fX = xx; }
inline void Vector3::SetY(double yy) { fY = yy; }
inline void Vector3::SetZ(double zz) { fZ = zz; }

inline void Vector3::SetXYZ(double xx, double yy, double zz) {
   fX = xx;
   fY = yy;
   fZ = zz;
}

inline void Vector3::GetXYZ(double *carray) const {
   carray[0] = fX;
   carray[1] = fY;
   carray[2] = fZ;
}

inline void Vector3::GetXYZ(float *carray) const {
   carray[0] = fX;
   carray[1] = fY;
   carray[2] = fZ;
}


inline Vector3 & Vector3::operator = (const Vector3 & p) {
   fX = p.fX;
   fY = p.fY;
   fZ = p.fZ;
   return *this;
}

inline bool Vector3::operator == (const Vector3& v) const {
   return (v.fX==fX && v.fY==fY && v.fZ==fZ) ? true : false;
}

inline bool Vector3::operator != (const Vector3& v) const {
   return (v.fX!=fX || v.fY!=fY || v.fZ!=fZ) ? true : false;
}

inline Vector3& Vector3::operator += (const Vector3 & p) {
   fX += p.fX;
   fY += p.fY;
   fZ += p.fZ;
   return *this;
}

inline Vector3& Vector3::operator -= (const Vector3 & p) {
   fX -= p.fX;
   fY -= p.fY;
   fZ -= p.fZ;
   return *this;
}

inline Vector3 Vector3::operator - () const {
   return Vector3(-fX, -fY, -fZ);
}

inline Vector3& Vector3::operator *= (double a) {
   fX *= a;
   fY *= a;
   fZ *= a;
   return *this;
}

inline double Vector3::Dot(const Vector3 & p) const {
   return fX*p.fX + fY*p.fY + fZ*p.fZ;
}

inline Vector3 Vector3::Cross(const Vector3 & p) const {
   return Vector3(fY*p.fZ-p.fY*fZ, fZ*p.fX-p.fZ*fX, fX*p.fY-p.fX*fY);
}

inline double Vector3::Mag2() const { return fX*fX + fY*fY + fZ*fZ; }


inline Vector3 Vector3::Orthogonal() const {
   double xx = fX < 0.0 ? -fX : fX;
   double yy = fY < 0.0 ? -fY : fY;
   double zz = fZ < 0.0 ? -fZ : fZ;
   if (xx < yy) {
      return xx < zz ? Vector3(0,fZ,-fY) : Vector3(fY,-fX,0);
   } else {
      return yy < zz ? Vector3(-fZ,0,fX) : Vector3(fY,-fX,0);
   }
}

inline double Vector3::Perp2() const { return fX*fX + fY*fY; }


inline double Vector3::Pt() const { return Perp(); }

inline double Vector3::Perp2(const Vector3 & p)  const {
   double tot = p.Mag2();
   double ss  = Dot(p);
   double per = Mag2();
   if (tot > 0.0) per -= ss*ss/tot;
   if (per < 0)   per = 0;
   return per;
}

inline double Vector3::Pt(const Vector3 & p) const {
   return Perp(p);
}

inline double Vector3::CosTheta() const {
   double ptot = Mag();
   return ptot == 0.0 ? 1.0 : fZ/ptot;
}

inline void Vector3::SetMag(double ma) {
   double factor = Mag();
   if (factor == 0) {
     std::cerr << "Vector3::SetMag(): zero vector can't be stretched" << std::endl;
   } else {
      factor = ma/factor;
      SetX(fX*factor);
      SetY(fY*factor);
      SetZ(fZ*factor);
   }
}

inline void Vector3::SetPerp(double r) {
   double p = Perp();
   if (p != 0.0) {
      fX *= r/p;
      fY *= r/p;
   }
}

inline double Vector3::DeltaPhi(const Vector3 & v) const {
   return Vector2::Phi_mpi_pi(Phi()-v.Phi());
}

inline double Vector3::Eta() const {
   return PseudoRapidity();
}

inline double Vector3::DrEtaPhi(const Vector3 & v) const{
   return DeltaR(v);
}


inline Vector2 Vector3::EtaPhiVector() const {
   return Vector2 (Eta(),Phi());
}

inline Vector2 Vector3::XYvector() const {
   return Vector2(fX,fY);
}

Vector3::Vector3(const Vector3 & p) : 
  fX(p.fX), fY(p.fY), fZ(p.fZ) {}

Vector3::Vector3(double xx, double yy, double zz)
: fX(xx), fY(yy), fZ(zz) {}

Vector3::Vector3(const double * x0)
: fX(x0[0]), fY(x0[1]), fZ(x0[2]) {}

Vector3::Vector3(const float * x0)
: fX(x0[0]), fY(x0[1]), fZ(x0[2]) {}

Vector3::~Vector3() {}


double Vector3::operator () (int i) const {
   //dereferencing operator const
   switch(i) {
      case 0:
         return fX;
      case 1:
         return fY;
      case 2:
         return fZ;
      default:
	std::cerr << "Vector3::operator()(i) bad index (" << i << "), returning 0" << std::endl;
   }
   return 0.;
}


double & Vector3::operator () (int i) {
   //dereferencing operator
   switch(i) {
      case 0:
         return fX;
      case 1:
         return fY;
      case 2:
         return fZ;
      default:
	std::cerr << "Vector3::operator()(i):bad index (" << i << ")" << ", returning &fX" << std::endl;
   }
   return fX;
}


Vector3 & Vector3::operator *= (const Rotation & m){
   //multiplication operator
  return *this = m * (*this);
}
 

Vector3 & Vector3::Transform(const Rotation & m) {
   //transform this vector with a Rotation
  // return *this = Vector3(m.XX()*fX + m.XY()*fY + m.XZ()*fZ,
  // 			 m.YX()*fX + m.YY()*fY + m.YZ()*fZ,
  // 			 m.ZX()*fX + m.ZY()*fY + m.ZZ()*fZ);

  return *this = m * (*this);
}
 


double Vector3::Angle(const Vector3 & q) const 
{
   // return the angle w.r.t. another 3-vector
   double ptot2 = Mag2()*q.Mag2();
   if(ptot2 <= 0) {
      return 0.0;
   } else {
      double arg = Dot(q)/sqrt(ptot2);
      if(arg >  1.0) arg =  1.0;
      if(arg < -1.0) arg = -1.0;
      return acos(arg);
   }
}


double Vector3::Mag() const 
{ 
   // return the magnitude (rho in spherical coordinate system)
   
   return sqrt(Mag2()); 
}


double Vector3::Perp() const 
{ 
   //return the transverse component  (R in cylindrical coordinate system)

   return sqrt(Perp2()); 
}



double Vector3::Perp(const Vector3 & p) const
{ 
   //return the transverse component (R in cylindrical coordinate system)

   return sqrt(Perp2(p)); 
}


double Vector3::Phi() const 
{
   //return the  azimuth angle. returns phi from -pi to pi
   return fX == 0.0 && fY == 0.0 ? 0.0 : atan2(fY,fX);
}


double Vector3::Theta() const 
{
   //return the polar angle
   return fX == 0.0 && fY == 0.0 && fZ == 0.0 ? 0.0 : atan2(Perp(),fZ);
}


Vector3 Vector3::Unit() const 
{
   // return unit vector parallel to this.
   double  tot = Mag2();
   Vector3 p(fX,fY,fZ);
   return tot > 0.0 ? p *= (1.0/sqrt(tot)) : p;
}


void Vector3::RotateX(double angle) {
   //rotate vector around X
   double s = sin(angle);
   double c = cos(angle);
   double yy = fY;
   fY = c*yy - s*fZ;
   fZ = s*yy + c*fZ;
}


void Vector3::RotateY(double angle) {
   //rotate vector around Y
   double s = sin(angle);
   double c = cos(angle);
   double zz = fZ;
   fZ = c*zz - s*fX;
   fX = s*zz + c*fX;
}


void Vector3::RotateZ(double angle) {
   //rotate vector around Z
   double s = sin(angle);
   double c = cos(angle);
   double xx = fX;
   fX = c*xx - s*fY;
   fY = s*xx + c*fY;
}


void Vector3::Rotate(double angle, const Vector3 & axis){
   //rotate vector
   Rotation trans;
   trans.Rotate(angle, axis);
   operator*=(trans);
}


void Vector3::RotateUz(const Vector3& NewUzVector) {
   // NewUzVector must be normalized !

   double u1 = NewUzVector.fX;
   double u2 = NewUzVector.fY;
   double u3 = NewUzVector.fZ;
   double up = u1*u1 + u2*u2;

   if (up) {
      up = sqrt(up);
      double px = fX,  py = fY,  pz = fZ;
      fX = (u1*u3*px - u2*py + u1*up*pz)/up;
      fY = (u2*u3*px + u1*py + u2*up*pz)/up;
      fZ = (u3*u3*px -    px + u3*up*pz)/up;
   } else if (u3 < 0.) { fX = -fX; fZ = -fZ; }      // phi=0  teta=pi
   else {};
}


double Vector3::PseudoRapidity() const {
   //double m = Mag();
   //return 0.5*log( (m+fZ)/(m-fZ) );
   // guard against Pt=0
   double cosTheta = CosTheta();
   if (cosTheta*cosTheta < 1) 
     return -0.5* log( (1.0-cosTheta)/(1.0+cosTheta) );
   std::cerr << "Vector3::PseudoRapidity(): transvers momentum = 0! return +/- 10e10" << std::endl;
   if (fZ > 0) return 10e10;
   else        return -10e10;
}

void Vector3::SetPtEtaPhi(double pt, double eta, double phi) {
   //set Pt, Eta and Phi
   double apt = fabs(pt);
   SetXYZ(apt*cos(phi), apt*sin(phi), apt/tan(2.0*atan(exp(-eta))) );
}

void Vector3::SetPtThetaPhi(double pt, double theta, double phi) {
   //set Pt, Theta and Phi
   fX = pt * cos(phi);
   fY = pt * sin(phi); 
   double tanTheta = tan(theta);
   fZ = tanTheta ? pt / tanTheta : 0;
}


void Vector3::SetTheta(double th) 
{
   // Set theta keeping mag and phi constant (BaBar).
   double ma   = Mag();
   double ph   = Phi();
   SetX(ma*sin(th)*cos(ph));
   SetY(ma*sin(th)*sin(ph));
   SetZ(ma*cos(th));
}


void Vector3::SetPhi(double ph) 
{
   // Set phi keeping mag and theta constant (BaBar).
   double xy   = Perp();
   SetX(xy*cos(ph));
   SetY(xy*sin(ph));
}


double Vector3::DeltaR(const Vector3 & v) const 
{
   //return deltaR with respect to v
   double deta = Eta()-v.Eta();
   double dphi = Vector2::Phi_mpi_pi(Phi()-v.Phi());
   return sqrt( deta*deta+dphi*dphi );
}

void Vector3::SetMagThetaPhi(double mag, double theta, double phi) 
{
   //setter with mag, theta, phi
   double amag = fabs(mag);
   fX = amag * sin(theta) * cos(phi);
   fY = amag * sin(theta) * sin(phi);
   fZ = amag * cos(theta);
}


Vector3 operator + (const Vector3 & a, const Vector3 & b) {
   return Vector3(a.X() + b.X(), a.Y() + b.Y(), a.Z() + b.Z());
}

Vector3 operator - (const Vector3 & a, const Vector3 & b) {
   return Vector3(a.X() - b.X(), a.Y() - b.Y(), a.Z() - b.Z());
}

Vector3 operator * (const Vector3 & p, double a) {
   return Vector3(a*p.X(), a*p.Y(), a*p.Z());
}

Vector3 operator * (double a, const Vector3 & p) {
   return Vector3(a*p.X(), a*p.Y(), a*p.Z());
}

double operator * (const Vector3 & a, const Vector3 & b) {
   return a.Dot(b);
}

std::string Vector3::ToString() {
  std::stringstream ss;
  ss << "Vector3(" << fX << "," << fY << "," << fZ << ")";
  return ss.str();
}

inline double Rotation::XX() const { return fxx; }
inline double Rotation::XY() const { return fxy; }
inline double Rotation::XZ() const { return fxz; }
inline double Rotation::YX() const { return fyx; }
inline double Rotation::YY() const { return fyy; }
inline double Rotation::YZ() const { return fyz; }
inline double Rotation::ZX() const { return fzx; }
inline double Rotation::ZY() const { return fzy; }
inline double Rotation::ZZ() const { return fzz; }

inline Rotation::RotationRow::RotationRow
(const Rotation & r, int i) : fRR(&r), fII(i) {}

inline Rotation::RotationRow::RotationRow
(const RotationRow & rr) : fRR(rr.fRR), fII(rr.fII) {}

inline Rotation::RotationRow & Rotation::RotationRow::operator = (const Rotation::RotationRow & rr) {
   fRR = rr.fRR;
   fII = rr.fII;
   return *this;
}

inline double Rotation::RotationRow::operator [] (int jj) const {
   return fRR->operator()(fII,jj);
}

inline Rotation::RotationRow Rotation::operator [] (int i) const {
   return RotationRow(*this, i);
}

inline Rotation & Rotation::operator = (const Rotation & m) {
   fxx = m.fxx;
   fxy = m.fxy;
   fxz = m.fxz;
   fyx = m.fyx;
   fyy = m.fyy;
   fyz = m.fyz;
   fzx = m.fzx;
   fzy = m.fzy;
   fzz = m.fzz;
   return *this;
}

inline bool Rotation::operator == (const Rotation& m) const {
   return (fxx == m.fxx && fxy == m.fxy && fxz == m.fxz &&
           fyx == m.fyx && fyy == m.fyy && fyz == m.fyz &&
           fzx == m.fzx && fzy == m.fzy && fzz == m.fzz) ? true : false;
}

inline bool Rotation::operator != (const Rotation &m) const {
   return (fxx != m.fxx || fxy != m.fxy || fxz != m.fxz ||
           fyx != m.fyx || fyy != m.fyy || fyz != m.fyz ||
           fzx != m.fzx || fzy != m.fzy || fzz != m.fzz) ? true : false;
}

inline bool Rotation::IsIdentity() const {
   return  (fxx == 1.0 && fxy == 0.0 && fxz == 0.0 &&
            fyx == 0.0 && fyy == 1.0 && fyz == 0.0 &&
            fzx == 0.0 && fzy == 0.0 && fzz == 1.0) ? true : false;
}

inline Rotation & Rotation::SetToIdentity() {
   fxx = fyy = fzz = 1.0;
   fxy = fxz = fyx = fyz = fzx = fzy = 0.0;
   return *this;
}

inline Vector3 Rotation::operator * (const Vector3 & p) const {
  return Vector3(fxx*p.X() + fxy*p.Y() + fxz*p.Z(),
		 fyx*p.X() + fyy*p.Y() + fyz*p.Z(),
		 fzx*p.X() + fzy*p.Y() + fzz*p.Z());
}

inline Rotation & Rotation::operator *= (const Rotation & m) {
   return *this = operator * (m);
}

inline Rotation & Rotation::Transform(const Rotation & m) {
   return *this = m.operator * (*this);
}

inline Rotation Rotation::Inverse() const {
   return Rotation(fxx, fyx, fzx, fxy, fyy, fzy, fxz, fyz, fzz);
}

inline Rotation & Rotation::Invert() {
   return *this=Inverse();
}

inline Rotation & Rotation::Rotate(double psi, const Vector3 * p) {
   return Rotate(psi, *p);
}

std::string Rotation::ToString() {
  std::stringstream ss;
  ss << "Rotation(" << std::setprecision(8) << fxx << " " << fxy << " " << fxz << std::endl;
  ss << "         " << std::setprecision(8) << fyx << " " << fyy << " " << fyz << std::endl;
  ss << "         " << std::setprecision(8) << fzx << " " << fzy << " " << fzz << ")";

  return ss.str();
}

#define TOLERANCE (1.0E-6) //swig ignore

Rotation::Rotation()
: fxx(1.0), fxy(0.0), fxz(0.0), fyx(0.0), fyy(1.0), fyz(0.0),
  fzx(0.0), fzy(0.0), fzz(1.0) {}

Rotation::Rotation(const Rotation & m) : 
  fxx(m.fxx), fxy(m.fxy), fxz(m.fxz), fyx(m.fyx), fyy(m.fyy), fyz(m.fyz),
  fzx(m.fzx), fzy(m.fzy), fzz(m.fzz) {}

Rotation::Rotation(double mxx, double mxy, double mxz,
                         double myx, double myy, double myz,
                         double mzx, double mzy, double mzz)
: fxx(mxx), fxy(mxy), fxz(mxz), fyx(myx), fyy(myy), fyz(myz),
  fzx(mzx), fzy(mzy), fzz(mzz) {}


double Rotation::operator() (int i, int j) const {
   //dereferencing operator const
   if (i == 0) {
      if (j == 0) { return fxx; }
      if (j == 1) { return fxy; }
      if (j == 2) { return fxz; }
   } else if (i == 1) {
      if (j == 0) { return fyx; }
      if (j == 1) { return fyy; }
      if (j == 2) { return fyz; }
   } else if (i == 2) {
      if (j == 0) { return fzx; }
      if (j == 1) { return fzy; }
      if (j == 2) { return fzz; }
   }
  
   std::cerr << "Rotation::operator()(i,j): bad indices (" << i << "," << j << ")" << std::endl;

   return 0.0;
}

Rotation Rotation::operator* (const Rotation & b) const {
   //multiplication operator
   return Rotation(fxx*b.fxx + fxy*b.fyx + fxz*b.fzx,
                    fxx*b.fxy + fxy*b.fyy + fxz*b.fzy,
                    fxx*b.fxz + fxy*b.fyz + fxz*b.fzz,
                    fyx*b.fxx + fyy*b.fyx + fyz*b.fzx,
                    fyx*b.fxy + fyy*b.fyy + fyz*b.fzy,
                    fyx*b.fxz + fyy*b.fyz + fyz*b.fzz,
                    fzx*b.fxx + fzy*b.fyx + fzz*b.fzx,
                    fzx*b.fxy + fzy*b.fyy + fzz*b.fzy,
                    fzx*b.fxz + fzy*b.fyz + fzz*b.fzz);
}


Rotation & Rotation::Rotate(double a, const Vector3& axis) {
   //rotate along an axis
   if (a != 0.0) {
      double ll = axis.Mag();
      if (ll == 0.0) {
         std::cerr << "Rotation::Rotate(angle,axis)  zero axis " << std::endl;
      } else {
         double sa = sin(a), ca = cos(a);
         double dx = axis.X()/ll, dy = axis.Y()/ll, dz = axis.Z()/ll;
         Rotation m(
             ca+(1-ca)*dx*dx,          (1-ca)*dx*dy-sa*dz,    (1-ca)*dx*dz+sa*dy,
             (1-ca)*dy*dx+sa*dz, ca+(1-ca)*dy*dy,          (1-ca)*dy*dz-sa*dx,
             (1-ca)*dz*dx-sa*dy,    (1-ca)*dz*dy+sa*dx, ca+(1-ca)*dz*dz );
         Transform(m);
      }
   }
   return *this;
}

Rotation & Rotation::RotateX(double a) {
   //rotate around x
   double c = cos(a);
   double s = sin(a);
   double x = fyx, y = fyy, z = fyz;
   fyx = c*x - s*fzx;
   fyy = c*y - s*fzy;
   fyz = c*z - s*fzz;
   fzx = s*x + c*fzx;
   fzy = s*y + c*fzy;
   fzz = s*z + c*fzz;
   return *this;
}

Rotation & Rotation::RotateY(double a){
   //rotate around y
   double c = cos(a);
   double s = sin(a);
   double x = fzx, y = fzy, z = fzz;
   fzx = c*x - s*fxx;
   fzy = c*y - s*fxy;
   fzz = c*z - s*fxz;
   fxx = s*x + c*fxx;
   fxy = s*y + c*fxy;
   fxz = s*z + c*fxz;
   return *this;
}

Rotation & Rotation::RotateZ(double a) {
   //rotate around z
   double c = cos(a);
   double s = sin(a);
   double x = fxx, y = fxy, z = fxz;
   fxx = c*x - s*fyx;
   fxy = c*y - s*fyy;
   fxz = c*z - s*fyz;
   fyx = s*x + c*fyx;
   fyy = s*y + c*fyy;
   fyz = s*z + c*fyz;
   return *this;
}

Rotation & Rotation::RotateAxes(const Vector3 &newX,
                                  const Vector3 &newY,
                                  const Vector3 &newZ) {
   //rotate axes
   double del = 0.001;
   Vector3 w = newX.Cross(newY);

   if (fabs(newZ.X()-w.X()) > del ||
       fabs(newZ.Y()-w.Y()) > del ||
       fabs(newZ.Z()-w.Z()) > del ||
       fabs(newX.Mag2()-1.) > del ||
       fabs(newY.Mag2()-1.) > del ||
       fabs(newZ.Mag2()-1.) > del ||
       fabs(newX.Dot(newY)) > del ||
       fabs(newY.Dot(newZ)) > del ||
       fabs(newZ.Dot(newX)) > del) {
      std::cerr << "Rotation::RotateAxes bad axis vectors " << std::endl;
      return *this;
   } else {
      return Transform(Rotation(newX.X(), newY.X(), newZ.X(),
                                 newX.Y(), newY.Y(), newZ.Y(),
                                 newX.Z(), newY.Z(), newZ.Z()));
   }
}

double Rotation::PhiX() const {
   //return Phi
   return (fyx == 0.0 && fxx == 0.0) ? 0.0 : atan2(fyx,fxx);
}

double Rotation::PhiY() const {
   //return Phi
   return (fyy == 0.0 && fxy == 0.0) ? 0.0 : atan2(fyy,fxy);
}

double Rotation::PhiZ() const {
   //return Phi
   return (fyz == 0.0 && fxz == 0.0) ? 0.0 : atan2(fyz,fxz);
}

double Rotation::ThetaX() const {
   //return Phi
   return acos(fzx);
}

double Rotation::ThetaY() const {
   //return Theta
   return acos(fzy);
}

double Rotation::ThetaZ() const {
   //return Theta
   return acos(fzz);
}

void Rotation::AngleAxis(double &angle, Vector3 &axis) const {
   //rotation defined by an angle and a vector
   double cosa  = 0.5*(fxx+fyy+fzz-1);
   double cosa1 = 1-cosa;
   if (cosa1 <= 0) {
      angle = 0;
      axis  = Vector3(0,0,1);
   } else {
      double x=0, y=0, z=0;
      if (fxx > cosa) x = sqrt((fxx-cosa)/cosa1);
      if (fyy > cosa) y = sqrt((fyy-cosa)/cosa1);
      if (fzz > cosa) z = sqrt((fzz-cosa)/cosa1);
      if (fzy < fyz)  x = -x;
      if (fxz < fzx)  y = -y;
      if (fyx < fxy)  z = -z;
      angle = acos(cosa);
      axis  = Vector3(x,y,z);
   }
}

Rotation & Rotation::SetXEulerAngles(double phi,
                                      double theta,
                                      double psi) {
   // Rotate using the x-convention (Landau and Lifshitz, Goldstein, &c) by 
   // doing the explicit rotations.  This is slightly less efficient than 
   // directly applying the rotation, but makes the code much clearer.  My
   // presumption is that this code is not going to be a speed bottle neck.

   SetToIdentity();
   RotateZ(phi);
   RotateX(theta);
   RotateZ(psi);
  
   return *this;
}

Rotation & Rotation::SetYEulerAngles(double phi,
                                       double theta,
                                       double psi) {
   // Rotate using the y-convention.
    
   SetToIdentity();
   RotateZ(phi);
   RotateY(theta);
   RotateZ(psi);
   return *this;
}

Rotation & Rotation::RotateXEulerAngles(double phi,
                                         double theta,
                                         double psi) {
   // Rotate using the x-convention.
   Rotation euler;
   euler.SetXEulerAngles(phi,theta,psi);
   return Transform(euler);
}

Rotation & Rotation::RotateYEulerAngles(double phi,
                                          double theta,
                                          double psi) {
   // Rotate using the y-convention.
   Rotation euler;
   euler.SetYEulerAngles(phi,theta,psi);
   return Transform(euler);
}

void Rotation::SetXPhi(double phi) {
   //set XPhi
   SetXEulerAngles(phi,GetXTheta(),GetXPsi());
}

void Rotation::SetXTheta(double theta) {
   //set XTheta
   SetXEulerAngles(GetXPhi(),theta,GetXPsi());
}

void Rotation::SetXPsi(double psi) {
   //set XPsi
   SetXEulerAngles(GetXPhi(),GetXTheta(),psi);
}

void Rotation::SetYPhi(double phi) {
   //set YPhi
   SetYEulerAngles(phi,GetYTheta(),GetYPsi());
}

void Rotation::SetYTheta(double theta) {
   //set YTheta
   SetYEulerAngles(GetYPhi(),theta,GetYPsi());
}

void Rotation::SetYPsi(double psi) {
   //set YPsi
   SetYEulerAngles(GetYPhi(),GetYTheta(),psi);
}

double Rotation::GetXPhi(void) const {
   //return phi angle
   double finalPhi;

   double s2 =  1.0 - fzz*fzz;
   if (s2 < 0) {
      std::cerr << "Rotation::GetPhi()  |fzz| > 1  " << std::endl;
      s2 = 0;
   }
   const double sinTheta = sqrt(s2);

   if (sinTheta != 0) {
      const double cscTheta = 1/sinTheta;
      double cosAbsPhi =  fzy * cscTheta;
      if ( fabs(cosAbsPhi) > 1 ) {        // NaN-proofing
         std::cerr << "Rotation::GetPhi() finds | cos phi | > 1 " << std::endl;
         cosAbsPhi = 1;
      }
      const double absPhi = acos(cosAbsPhi);
      if (fzx > 0) {
         finalPhi = absPhi;
      } else if (fzx < 0) {
         finalPhi = -absPhi;
      } else if (fzy > 0) {
         finalPhi = 0.0;
      } else {
         finalPhi = M_PI;
      }
   } else {              // sinTheta == 0 so |Fzz| = 1
      const double absPhi = .5 * acos (fxx);
      if (fxy > 0) {
         finalPhi =  -absPhi;
      } else if (fxy < 0) {
         finalPhi =   absPhi;
      } else if (fxx>0) {
         finalPhi = 0.0;
      } else {
         finalPhi = fzz * M_PI/2;
      }
   }
   return finalPhi;
}

double Rotation::GetYPhi(void) const {
   //return YPhi
   return GetXPhi() + M_PI/2.0;
}

double Rotation::GetXTheta(void) const {
   //return XTheta
   return  ThetaZ();
}

double Rotation::GetYTheta(void) const {
   //return YTheta
   return  ThetaZ();
}

double Rotation::GetXPsi(void) const {
   //Get psi angle
   double finalPsi = 0.0;

   double s2 =  1.0 - fzz*fzz;
   if (s2 < 0) {
      std::cerr << "Rotation::GetPsi()  |fzz| > 1  " << std::endl;
      s2 = 0;
   }
   const double sinTheta = sqrt(s2);

   if (sinTheta != 0) {
      const double cscTheta = 1/sinTheta;
      double cosAbsPsi =  - fyz * cscTheta;
      if ( fabs(cosAbsPsi) > 1 ) {        // NaN-proofing
         std::cerr << "Rotation::GetPsi() | cos psi | > 1  " << std::endl;
         cosAbsPsi = 1;
      }
      const double absPsi = acos(cosAbsPsi);
      if (fxz > 0) {
         finalPsi = absPsi;
      } else if (fxz < 0) {
         finalPsi = -absPsi;
      } else {
         finalPsi = (fyz < 0) ? 0 : M_PI;
      }
   } else {              // sinTheta == 0 so |Fzz| = 1
      double absPsi = fxx;
      if ( fabs(fxx) > 1 ) {        // NaN-proofing
         std::cerr << "Rotation::GetPsi() | fxx | > 1  " << std::endl;
         absPsi = 1;
      }
      absPsi = .5 * acos (absPsi);
      if (fyx > 0) {
         finalPsi = absPsi;
      } else if (fyx < 0) {
         finalPsi = -absPsi;
      } else {
         finalPsi = (fxx > 0) ? 0 : M_PI/2;
      }
   }
   return finalPsi;
}

double Rotation::GetYPsi(void) const {
   //return YPsi
   return GetXPsi() - M_PI/2;
}

Rotation & Rotation::SetXAxis(const Vector3& axis, 
                                const Vector3& xyPlane) {
   //set X axis
   Vector3 xAxis(xyPlane);
   Vector3 yAxis;
   Vector3 zAxis(axis);
   MakeBasis(xAxis,yAxis,zAxis);
   fxx = zAxis.X();  fyx = zAxis.Y();  fzx = zAxis.Z();
   fxy = xAxis.X();  fyy = xAxis.Y();  fzy = xAxis.Z();
   fxz = yAxis.X();  fyz = yAxis.Y();  fzz = yAxis.Z();
   return *this;
}

Rotation & Rotation::SetXAxis(const Vector3& axis) {
   //set X axis
   Vector3 xyPlane(0.0,1.0,0.0);
   return SetXAxis(axis,xyPlane);
}

Rotation & Rotation::SetYAxis(const Vector3& axis, 
                                const Vector3& yzPlane) {
   //set Y axis
   Vector3 xAxis(yzPlane);
   Vector3 yAxis;
   Vector3 zAxis(axis);
   MakeBasis(xAxis,yAxis,zAxis);
   fxx = yAxis.X();  fyx = yAxis.Y();  fzx = yAxis.Z();
   fxy = zAxis.X();  fyy = zAxis.Y();  fzy = zAxis.Z();
   fxz = xAxis.X();  fyz = xAxis.Y();  fzz = xAxis.Z();
   return *this;
}

Rotation & Rotation::SetYAxis(const Vector3& axis) {
   //set Y axis
   Vector3 yzPlane(0.0,0.0,1.0);
   return SetYAxis(axis,yzPlane);
}

Rotation & Rotation::SetZAxis(const Vector3& axis, 
                                const Vector3& zxPlane) {
   //set Z axis
   Vector3 xAxis(zxPlane);
   Vector3 yAxis;
   Vector3 zAxis(axis);
   MakeBasis(xAxis,yAxis,zAxis);
   fxx = xAxis.X();  fyx = xAxis.Y();  fzx = xAxis.Z();
   fxy = yAxis.X();  fyy = yAxis.Y();  fzy = yAxis.Z();
   fxz = zAxis.X();  fyz = zAxis.Y();  fzz = zAxis.Z();
   return *this;
}

Rotation & Rotation::SetZAxis(const Vector3& axis) {
   //set Z axis
   Vector3 zxPlane(1.0,0.0,0.0);
   return SetZAxis(axis,zxPlane);
}

void Rotation::MakeBasis(Vector3& xAxis,
                          Vector3& yAxis,
                          Vector3& zAxis) const {
   // Make the Z axis into a unit variable. 
   double zmag = zAxis.Mag();
   if (zmag<TOLERANCE) {
      std::cerr << "Rotation::MakeBasis(X,Y,Z) non-zero Z Axis is required " << std::endl;
   }
   zAxis *= (1.0/zmag);

   double xmag = xAxis.Mag();
   if (xmag<TOLERANCE*zmag) {
      xAxis = zAxis.Orthogonal();
      xmag = 1.0;
   }

   // Find the Y axis
   yAxis = zAxis.Cross(xAxis)*(1.0/xmag);
   double ymag = yAxis.Mag();
   if (ymag<TOLERANCE*zmag) {
      yAxis = zAxis.Orthogonal();
   } else {
      yAxis *= (1.0/ymag);
   }

   xAxis = yAxis.Cross(zAxis);
}

#endif
